#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
bool SELT_TEST_RESULT = false;
int SIM_MODE = 1;
int USE_ROBOT_FRAME = 0;
int FOLLOW_WALL = 1;
int DataSet::clean_mode = WAIT_TIME;               // 当前的清洁模式，如导航、沿边
int DataSet::ctrl_state = MAP_POSE_CHECK_PLAN;     // MAP_POSE_CHECK_PLAN
int DataSet::ctrl_state_sub = NAVI_TO_POSE_PLAN;
int DataSet::clean_mode_suspend = WAIT_TIME;       // 当前的清洁模式，如导航、沿边
int DataSet::ctrl_state_suspend = WAIT_TIME;
int DataSet::ctrl_state_sub_suspend = NAVI_TO_POSE_PLAN;
double ROBOT_RADIUS = 0.15;     // 机器人机身半径
double RADIUS_SQUA = 0.0225;
double ROBOT_DIAMETER = 0.3;    // 机器人机身直径
double DIAMETER_SQUA = 0.09;
//
double MAX_TILT = 0.069813168;  // 触发倾角设定,弧度。 4度= 4*3.1415926/180
double MIN_TILT = 0.034906584;  // 去除触发倾角设定,弧度。 2度= 3*3.1415926/180
double MID_TILT = 0.052359876;  // 去除触发倾角设定,弧度。 3度= 2*3.1415926/180
//调试变量
double CLIFF_MAXDIST = 0.04;
double CLIFF_OBSDIST = 0.02;    // 需要控制的机器人与悬崖的距离
//调试变量
double MARGIN_MAXDIST = 0.8;    // 沿边传感器的最大距离
double MARGIN_TURNDIST = 0.5;
double MARGIN_LEFT = 0.05;
double MARGIN_RIGHT = 0.02;
double MARGIN_OBSDIST = 0.1;    // 需要控制的与障碍物的距离
double MARGIN_P = 2.7;
double MARGIN_I = 0.0;
double MARGIN_D = 0.3;
// 调试变量
double t_linear1 = 0.2;
double t_linear2 = 0.2;
double t_linear3 = 0.2;
double t_angular1 = -1.0;
double t_angular2 = -0.6;
double t_angular3 = -0.2;

// 机器人初始位置
PoseF DataSet::robot_pose = PoseF(0, 0, 0);
// 激光雷达在机器人上的位置
PoseF lidar_pose(0, 0, 0.0);
PoseF camera_pose(0.16f, 0.0f, .0f);
// 沿边传感器
PoseF margin_pose(0.0f, -0.16f, -0.523598767);
// 探地传感器在机器人上的位置
PoseF left_fron_cliff(0.125573684, 0.0725, 0.0);
PoseF left_mid_cliff(0.0, 0.145, 0.0);
PoseF left_back_cliff(-0.125573684, 0.0725, 0.0);
PoseF righ_fron_cliff(0.125573684, -0.0725, 0.0);
PoseF righ_mid_cliff(0.0, -0.145, 0.0);
PoseF righ_back_cliff(-0.125573684, -0.0725, 0.0);


void updateState(int clean_mode, int ctrl_state, int ctrl_state_sub, int clean_mode_suspend,
        int ctrl_state_suspend, int ctrl_state_sub_suspend)
{
    if (clean_mode != -1) DataSet::clean_mode = clean_mode;
    if (ctrl_state != -1) DataSet::ctrl_state = ctrl_state;
    if (ctrl_state_sub != -1) DataSet::ctrl_state_sub = ctrl_state_sub;
    if (clean_mode_suspend != -1) DataSet::clean_mode_suspend = clean_mode_suspend;
    if (ctrl_state_suspend != -1) DataSet::ctrl_state_suspend = ctrl_state_suspend;
    if (ctrl_state_sub_suspend != -1) DataSet::ctrl_state_sub_suspend = ctrl_state_sub_suspend;
};

geometry_msgs::msg::PoseStamped getRobotPose()
{
    geometry_msgs::msg::PoseStamped pose;
    pose.header.frame_id = "map";
    tf2::Quaternion quaternion_;
    quaternion_.setRPY(0, 0, DataSet::robot_pose.angle());
    pose.pose.position.x = DataSet::robot_pose.x();
    pose.pose.position.y = DataSet::robot_pose.y();
    pose.pose.orientation.x = quaternion_.x();
    pose.pose.orientation.y = quaternion_.y();
    pose.pose.orientation.z = quaternion_.z();
    pose.pose.orientation.w = quaternion_.w();
    return pose;
}
}